#include "stdafx.h"

#include "LocatorPoseMex.h"

void runBundleAdjustment(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
	if (nrhs != 29) mexErrMsgTxt("BundleAdjustment needs 28 arguments.");
	if ( (mxGetM(prhs[1]) != 3) || (mxGetN(prhs[1]) != 1)) mexErrMsgTxt("x0 must be a 3x1 vector.");
	if ( (mxGetM(prhs[2]) != 2) || (mxGetN(prhs[2]) != 1)) mexErrMsgTxt("centerA must be a 2x1 vector.");
	if ( (mxGetM(prhs[3]) != 2) || (mxGetN(prhs[3]) != 1)) mexErrMsgTxt("centerB must be a 2x1 vector.");
	if ( (mxGetM(prhs[4]) != 2) || (mxGetN(prhs[4]) != 1)) mexErrMsgTxt("sA must be a 2x1 vector.");
	if ( (mxGetM(prhs[5]) != 2) || (mxGetN(prhs[5]) != 1)) mexErrMsgTxt("sB must be a 2x1 vector.");
	
	for (int i = 6; i < 28; i++)
		if( (mxGetM(prhs[i]) != 2) || (mxGetN(prhs[i]) != 1))
			mexErrMsgTxt("All calibration parameters must be a 2x1 vectors.");
		
	arma::colvec x0		 = convertMatlabToArma(prhs[1]);
	arma::colvec centerA = convertMatlabToArma(prhs[2]);
	arma::colvec centerB = convertMatlabToArma(prhs[3]);
	arma::colvec sA		 = convertMatlabToArma(prhs[4]);
	arma::colvec sB		 = convertMatlabToArma(prhs[5]);
	arma::colvec c		 = convertMatlabToArma(prhs[6]);
	arma::colvec a1		 = convertMatlabToArma(prhs[7]);
	arma::colvec a2		 = convertMatlabToArma(prhs[8]);
	arma::colvec a3		 = convertMatlabToArma(prhs[9]);
	arma::colvec b1		 = convertMatlabToArma(prhs[10]);
	arma::colvec b2		 = convertMatlabToArma(prhs[11]);
	arma::colvec c1		 = convertMatlabToArma(prhs[12]);
	arma::colvec c2		 = convertMatlabToArma(prhs[13]);
	arma::colvec ppx	 = convertMatlabToArma(prhs[14]);
	arma::colvec ppy	 = convertMatlabToArma(prhs[15]);
	arma::colvec r0		 = convertMatlabToArma(prhs[16]);
	arma::colvec esx	 = convertMatlabToArma(prhs[17]);
	arma::colvec esy	 = convertMatlabToArma(prhs[18]);
	arma::colvec ssx	 = convertMatlabToArma(prhs[19]);
	arma::colvec ssy	 = convertMatlabToArma(prhs[20]);
	arma::colvec ry1	 = convertMatlabToArma(prhs[21]);
	arma::colvec rx		 = convertMatlabToArma(prhs[22]);
	arma::colvec ry2	 = convertMatlabToArma(prhs[23]);
	arma::colvec rz		 = convertMatlabToArma(prhs[24]);
	arma::colvec tx		 = convertMatlabToArma(prhs[25]);
	arma::colvec ty		 = convertMatlabToArma(prhs[26]);
	arma::colvec tz		 = convertMatlabToArma(prhs[27]);
	int	max_iterations   = mxGetPr(prhs[28])[0];
	
	arma::colvec xout;
	arma::colvec sout;
	
	OTPF::Marker::bundleAdjustment(xout, sout, x0, centerA, centerB, sA, sB, c, a1, a2, a3, b1, b2, c1, c2, ppx,ppy, r0, esx, esy, ssx, ssy, ry1, rx, ry2, rz, tx, ty, tz, max_iterations);
	
	plhs[0] = convertArmaToMatlab(xout);
	plhs[1] = convertArmaToMatlab(sout);
	
}
